#include <avr/wdt.h>
#include <avr/sleep.h>
#include <string.h>

#include "portdef.h"
#include "usart_utils.h"
#include "parser.h"
#include "interface.h"
#include "limit_switch.h"
#include "helper.h"



/*******************************************************************/

/*
In the timer interrupt function, the waittime for each axis
is decremented since time has passed.
*/

#ifdef FULLSTEP
volatile uint8_t Preload=180;
#else
volatile uint8_t Preload=180;
#endif


void TIMER_Init() {

	TCNT0 = Preload;
	TCCR0B = (1<<CS01) | (1<<CS00);	// Prescaler = 64
	TIMSK0 |= (1<<TOIE0);
}

ISR(TIMER0_OVF_vect) {

	/* In the timer interrupt function, the waittime for each
	 * axis is decremented since time has passed.
	 */

	TIMSK0 &= ~(1<<TOIE0);
	if (mindelay > 0) {
		mindelay--;
	}
	TIMSK0 |= (1<<TOIE0);
	TCNT0 = Preload;
}

/*******************************************************************/

int main (void)
{

	// Received command is stored here
	char command[24];

	// Disable JTAG debugger functions.
	int i;
	for (i=0; i<8; i++) {
		MCUCR = 0x80;
	}

	// Power saving
	ACSR |= (1<<ACD);	// Disable Analog Comparator
	sleep_enable();
	set_sleep_mode(SLEEP_MODE_IDLE);

	// Initialize axis structures
	X.iMotor = 1;	// PB3
	Y.iMotor = 2;	// PB4
	Z.iMotor = 3;	// PB5

	X.iSwitch = 4;	// PD4
	Y.iSwitch = 5;	// PD5
 
	X.Pos = 0;
	Y.Pos = 0;
	Z.Pos = 0;

	// Outputs
	DDRC = _BV(A1) |_BV(A2) | _BV(A3) | _BV(A4);						// Data pins to L293Ds
	DDRB = _BV((X.iMotor+2)) | _BV((Y.iMotor+2)) | _BV((Z.iMotor+2));	// Enable Pins on L293Ds

	// Inputs
	DDRD = 0x00;	// End switch inputs PD0 and PD1

	// Initialize timer
	TIMER_Init();
 
	// Initialize USART
	USART_Init();

	// Initialize the user interface
	initInterface();
	changeStatus(STATUS_OK);

	// Initialize the limit switch
	initLimits();

	sei();

	/////MAIN LOOP/////////////MAIN LOOP///////////MAIN LOOP///

	while(1) {

		// Disable Timer
		TIMSK0 &= ~(1<<TOIE0);
		// Save power, turn off L293Ds
		PORTB &= ~(_BV((X.iMotor+2)) | _BV((Y.iMotor+2)) | _BV((Z.iMotor+2)));
		// Read command
		USART_Recive_String(command);
		// Enable Timer
		TIMSK0 |= (1<<TOIE0);
		// Parse command
		executeCommand(command);

	}

	/////MAIN LOOP/////////////MAIN LOOP///////////MAIN LOOP///

}
